#pragma once
#include "motion/rtcmds.h"
#include "zuc/nml_intf/zucpos.h"
#include "zuc/motion/motion.h"

#define FORCE_CONTROL_CMD_CLASS_NAME(x) CMD_CLASS_NAME(x) "[FCT CMD]"

/*****************************************************************************
 * 力控相关的指令
******************************************************************************/

class CmdSetAdmittanceEna : public mot::MultiRobCmd
{
public:
    CmdSetAdmittanceEna(int grp_id, double enable)
    {
        group_id_ = grp_id;
        cmd_name_ = FORCE_CONTROL_CMD_CLASS_NAME(CmdSetAdmittanceEna);
        enable_ = enable;
    };
    ~CmdSetAdmittanceEna() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double enable_;
};

class CmdSetCompliantType : public mot::MultiRobCmd
{
public:
    CmdSetCompliantType(int grp_id, int enable, int compliantType, int compliantEnable)
    {
        group_id_ = grp_id;
        cmd_name_ = FORCE_CONTROL_CMD_CLASS_NAME(CmdSetCompliantType);
        enable_ = enable;
        compliantType_ = compliantType;
        compliantEnable_ = compliantEnable;
    };
    ~CmdSetCompliantType() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int enable_;
    int compliantType_;
    int compliantEnable_;
};

class CmdSetAdmittanceOpt : public mot::MultiRobCmd
{
public:
    CmdSetAdmittanceOpt(int grp_id, const int* admit_axis_opt)
    {
        for (int i = 0; i < 6; i++) { admit_axis_opt_[i] = admit_axis_opt[i]; }
    };
    ~CmdSetAdmittanceOpt() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int admit_axis_opt_[6];
};

class CmdSetAdmittanceConfig : public mot::MultiRobCmd
{
public:
    CmdSetAdmittanceConfig(int grp_id, int dir, const FtConfig& ftConfig)
    {
        group_id_ = grp_id;
        cmd_name_ = FORCE_CONTROL_CMD_CLASS_NAME(CmdSetAdmittanceConfig);
        ftConfig_ = ftConfig;
        dir_ = dir;
    };
    ~CmdSetAdmittanceConfig() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int dir_;
    FtConfig ftConfig_;
};

class CmdSetVelCompliantCtrl : public mot::MultiRobCmd
{
public:
    CmdSetVelCompliantCtrl(int grp_id, const double* velCompliantCtrl)
    {
        group_id_ = grp_id;
        cmd_name_ = FORCE_CONTROL_CMD_CLASS_NAME(CmdSetVelCompliantCtrl);
        for (int i = 0; i < 5; i++) { velCompliantCtrl_[i] = velCompliantCtrl[i]; }
    };
    ~CmdSetVelCompliantCtrl() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double velCompliantCtrl_[5];
};

class CmdSetAdmittanceCtrlFrame : public mot::MultiRobCmd
{
public:
    CmdSetAdmittanceCtrlFrame(int grp_id, int ftFrame)
    {
        group_id_ = grp_id;
        cmd_name_ = FORCE_CONTROL_CMD_CLASS_NAME(CmdSetAdmittanceCtrlFrame);
        ftFrame_ = ftFrame;
    };
    ~CmdSetAdmittanceCtrlFrame() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    int ftFrame_;
};

class CmdDisableForceControl : public mot::MultiRobCmd
{
public:
    CmdDisableForceControl(int grp_id, double disableForceCtrl)
    {
        group_id_ = grp_id;
        cmd_name_ = FORCE_CONTROL_CMD_CLASS_NAME(CmdDisableForceControl);
        disableForceCtrl_ = disableForceCtrl;
    };
    ~CmdDisableForceControl() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double disableForceCtrl_;
};

class CmdSetFtCompliantCondition : public mot::MultiRobCmd
{
public:
    CmdSetFtCompliantCondition(int grp_id, const double* compliantCondition)
    {
        group_id_ = grp_id;
        cmd_name_ = FORCE_CONTROL_CMD_CLASS_NAME(CmdSetFtCompliantCondition);
        for (int i = 0; i < 6; i++) { compliantCondition_[i] = compliantCondition[i]; }
    };
    ~CmdSetFtCompliantCondition() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double compliantCondition_[6];
};

class CmdSetpayloadTool : public mot::MultiRobCmd
{
public:
    CmdSetpayloadTool(int grp_id, const Payload& payload)
    {
        group_id_ = grp_id;
        cmd_name_ = FORCE_CONTROL_CMD_CLASS_NAME(CmdSetpayloadTool);
        payload_ = payload;
    };
    ~CmdSetpayloadTool() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    Payload payload_;
};

class CmdSettorqueSensorSoftLimit : public mot::MultiRobCmd
{
public:
    CmdSettorqueSensorSoftLimit(int grp_id, const double* torqueSensorSoftLimit)
    {
        group_id_ = grp_id;
        cmd_name_ = FORCE_CONTROL_CMD_CLASS_NAME(CmdSettorqueSensorSoftLimit);
        for (int i = 0; i < 6; i++) { torqueSensorSoftLimit_[i] = torqueSensorSoftLimit[i]; }
    };
    ~CmdSettorqueSensorSoftLimit() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double torqueSensorSoftLimit_[6];
};

class CmdSetadmittanceDragDeadZone : public mot::MultiRobCmd
{
public:
    CmdSetadmittanceDragDeadZone(int grp_id, const double* admittanceDragDeadZone)
    {
        group_id_ = grp_id;
        cmd_name_ = FORCE_CONTROL_CMD_CLASS_NAME(CmdSetadmittanceDragDeadZone);
        for (int i = 0; i < 6; i++) { admittanceDragDeadZone_[i] = admittanceDragDeadZone[i]; }
    };
    ~CmdSetadmittanceDragDeadZone() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double admittanceDragDeadZone_[6];
};

class CmdSettorqueSensorFilter : public mot::MultiRobCmd
{
public:
    CmdSettorqueSensorFilter(int grp_id, double torqueSensorFilter)
    {
        group_id_ = grp_id;
        cmd_name_ = FORCE_CONTROL_CMD_CLASS_NAME(CmdSettorqueSensorFilter);
        torqueSensorFilter_ = torqueSensorFilter;
    };
    ~CmdSettorqueSensorFilter() {}

    virtual int active_callback() { return 0; };
    virtual int process();
    virtual int check_para() { return 0; };

    double torqueSensorFilter_;
};